#include "misc.h"
#include <math.h>

#define SWAPWORD(val) (((val&0x00FF)<<8)+((val&0xFF00)>>8))

extern byte Angle_Data[];

long v_random;

byte v_oscillate[66];
extern byte baselines[];

byte Sine_Data[] = {
0x00,0x00,0x00,0x06,0x00,0x0c,0x00,0x12,0x00,0x19,0x00,0x1f,0x00,0x25,0x00,
0x2b,0x00,0x31,0x00,0x38,0x00,0x3e,0x00,0x44,0x00,0x4a,0x00,0x50,0x00,0x56,
0x00,0x5c,0x00,0x61,0x00,0x67,0x00,0x6d,0x00,0x73,0x00,0x78,0x00,0x7e,0x00,
0x83,0x00,0x88,0x00,0x8e,0x00,0x93,0x00,0x98,0x00,0x9d,0x00,0xa2,0x00,0xa7,
0x00,0xab,0x00,0xb0,0x00,0xb5,0x00,0xb9,0x00,0xbd,0x00,0xc1,0x00,0xc5,0x00,
0xc9,0x00,0xcd,0x00,0xd1,0x00,0xd4,0x00,0xd8,0x00,0xdb,0x00,0xde,0x00,0xe1,
0x00,0xe4,0x00,0xe7,0x00,0xea,0x00,0xec,0x00,0xee,0x00,0xf1,0x00,0xf3,0x00,
0xf4,0x00,0xf6,0x00,0xf8,0x00,0xf9,0x00,0xfb,0x00,0xfc,0x00,0xfd,0x00,0xfe,
0x00,0xfe,0x00,0xff,0x00,0xff,0x00,0xff,0x01,0x00,0x00,0xff,0x00,0xff,0x00,
0xff,0x00,0xfe,0x00,0xfe,0x00,0xfd,0x00,0xfc,0x00,0xfb,0x00,0xf9,0x00,0xf8,
0x00,0xf6,0x00,0xf4,0x00,0xf3,0x00,0xf1,0x00,0xee,0x00,0xec,0x00,0xea,0x00,
0xe7,0x00,0xe4,0x00,0xe1,0x00,0xde,0x00,0xdb,0x00,0xd8,0x00,0xd4,0x00,0xd1,
0x00,0xcd,0x00,0xc9,0x00,0xc5,0x00,0xc1,0x00,0xbd,0x00,0xb9,0x00,0xb5,0x00,
0xb0,0x00,0xab,0x00,0xa7,0x00,0xa2,0x00,0x9d,0x00,0x98,0x00,0x93,0x00,0x8e,
0x00,0x88,0x00,0x83,0x00,0x7e,0x00,0x78,0x00,0x73,0x00,0x6d,0x00,0x67,0x00,
0x61,0x00,0x5c,0x00,0x56,0x00,0x50,0x00,0x4a,0x00,0x44,0x00,0x3e,0x00,0x38,
0x00,0x31,0x00,0x2b,0x00,0x25,0x00,0x1f,0x00,0x19,0x00,0x12,0x00,0x0c,0x00,
0x06,0x00,0x00,0xff,0xfa,0xff,0xf4,0xff,0xee,0xff,0xe7,0xff,0xe1,0xff,0xdb,
0xff,0xd5,0xff,0xcf,0xff,0xc8,0xff,0xc2,0xff,0xbc,0xff,0xb6,0xff,0xb0,0xff,
0xaa,0xff,0xa4,0xff,0x9f,0xff,0x99,0xff,0x93,0xff,0x8b,0xff,0x88,0xff,0x82,
0xff,0x7d,0xff,0x78,0xff,0x72,0xff,0x6d,0xff,0x68,0xff,0x63,0xff,0x5e,0xff,
0x59,0xff,0x55,0xff,0x50,0xff,0x4b,0xff,0x47,0xff,0x43,0xff,0x3f,0xff,0x3b,
0xff,0x37,0xff,0x33,0xff,0x2f,0xff,0x2c,0xff,0x28,0xff,0x25,0xff,0x22,0xff,
0x1f,0xff,0x1c,0xff,0x19,0xff,0x16,0xff,0x14,0xff,0x12,0xff,0x0f,0xff,0x0d,
0xff,0x0c,0xff,0x0a,0xff,0x08,0xff,0x07,0xff,0x05,0xff,0x04,0xff,0x03,0xff,
0x02,0xff,0x02,0xff,0x01,0xff,0x01,0xff,0x01,0xff,0x00,0xff,0x01,0xff,0x01,
0xff,0x01,0xff,0x02,0xff,0x02,0xff,0x03,0xff,0x04,0xff,0x05,0xff,0x07,0xff,
0x08,0xff,0x0a,0xff,0x0c,0xff,0x0d,0xff,0x0f,0xff,0x12,0xff,0x14,0xff,0x16,
0xff,0x19,0xff,0x1c,0xff,0x1f,0xff,0x22,0xff,0x25,0xff,0x28,0xff,0x2c,0xff,
0x2f,0xff,0x33,0xff,0x37,0xff,0x3b,0xff,0x3f,0xff,0x43,0xff,0x47,0xff,0x4b,
0xff,0x50,0xff,0x55,0xff,0x59,0xff,0x5e,0xff,0x63,0xff,0x68,0xff,0x6d,0xff,
0x72,0xff,0x78,0xff,0x7d,0xff,0x82,0xff,0x88,0xff,0x8b,0xff,0x93,0xff,0x99,
0xff,0x9f,0xff,0xa4,0xff,0xaa,0xff,0xb0,0xff,0xb6,0xff,0xbc,0xff,0xc2,0xff,
0xc8,0xff,0xcf,0xff,0xd5,0xff,0xdb,0xff,0xe1,0xff,0xe7,0xff,0xee,0xff,0xf4,
0xff,0xfa,0x00,0x00,0x00,0x06,0x00,0x0c,0x00,0x12,0x00,0x19,0x00,0x1f,0x00,
0x25,0x00,0x2b,0x00,0x31,0x00,0x38,0x00,0x3e,0x00,0x44,0x00,0x4a,0x00,0x50,
0x00,0x56,0x00,0x5c,0x00,0x61,0x00,0x67,0x00,0x6d,0x00,0x73,0x00,0x78,0x00,
0x7e,0x00,0x83,0x00,0x88,0x00,0x8e,0x00,0x93,0x00,0x98,0x00,0x9d,0x00,0xa2,
0x00,0xa7,0x00,0xab,0x00,0xb0,0x00,0xb5,0x00,0xb9,0x00,0xbd,0x00,0xc1,0x00,
0xc5,0x00,0xc9,0x00,0xcd,0x00,0xd1,0x00,0xd4,0x00,0xd8,0x00,0xdb,0x00,0xde,
0x00,0xe1,0x00,0xe4,0x00,0xe7,0x00,0xea,0x00,0xec,0x00,0xee,0x00,0xf1,0x00,
0xf3,0x00,0xf4,0x00,0xf6,0x00,0xf8,0x00,0xf9,0x00,0xfb,0x00,0xfc,0x00,0xfd,
0x00,0xfe,0x00,0xfe,0x00,0xff,0x00,0xff,0x00,0xff
};

word CalcAngle(signed short xdist, signed short ydist)
{
//; ---------------------------------------------------------------------------
//; Subroutine calculate an angle

//; input:
//;	d1 = x-axis distance
//;	d2 = y-axis distance

//; output:
//;	d0 = angle
//; ---------------------------------------------------------------------------

//; ||||||||||||||| S U B	R O U T	I N E |||||||||||||||||||||||||||||||||||||||

    signed short angle; 

    //CalcAngle:
    //movem.l	d3-d4,-(sp)
    //moveq	#0,d3
    //moveq	#0,d4
    //move.w	d1,d3
    signed short _xdist = xdist;

    //move.w	d2,d4
    signed short _ydist = ydist;

    //or.w	d3,d4
    //beq.s	loc_2D04
    if ((_xdist | _ydist) != 0)
    {
        //move.w	d2,d4
	//tst.w	d3
        //bpl.w	loc_2CC2
	//neg.w	d3
        if (_xdist < 0) { _xdist = -_xdist; }

        //loc_2CC2:
	//tst.w	d4
	//bpl.w	loc_2CCA
	//neg.w	d4
        if (_ydist < 0) { _ydist = -ydist; }

        //loc_2CCA:
	//cmp.w	d3,d4
        //bcc.w	loc_2CDC
        if (_ydist < _xdist)
        {
             //lsl.l	#8,d4
            long ydistScale = _ydist * 256;             //these 2 lines are equivalent to using the distance formula
	   
            //divu.w	d3,d4
            word angleIndex = ydistScale / _xdist; //i.e. (ydist/xdist)*256, (256 corresponding angle entrys)

	    //moveq	#0,d0
	    //move.b	Angle_Data(pc,d4.w),d0
            angle = Angle_Data[angleIndex];

            //bra.s	loc_2CE6
        }
        else
        {
            //loc_2CDC:
	    //lsl.l	#8,d3
            long xdistScale = _xdist * 256;

	    //divu.w	d4,d3
            word angleIndex = xdistScale / _ydist;

	    //moveq	#$40,d0
	    //sub.b	Angle_Data(pc,d3.w),d0
            angle = Angle_Data[angleIndex] - 0x40;
        }

        //loc_2CE6:
	//tst.w	d1
        //bpl.w	loc_2CF2
	//neg.w	d0
        //addi.w	#$80,d0
        if (xdist > 0) { angle = -angle + 0x80; }

	//loc_2CF2:
	//tst.w	d2
	//bpl.w	loc_2CFE
	//neg.w	d0
	//addi.w	#$100,d0
        if (ydist > 0) { angle = -angle + 0x100; }

        //loc_2CFE:
	//movem.l	(sp)+,d3-d4
	//rts	
    }
    //; ===========================================================================
    else
    {
        //loc_2D04:
        //move.w	#$40,d0
        angle = 0x40;
	
        //movem.l	(sp)+,d3-d4
	//rts
    }
    return angle; 	
    
}    //; End of function CalcAngle

void CalcSine(word angle, signed short &sine, signed short &cosine)
{
     angle=((angle&0xFF)*2);
     word cosval = (*(word*)(&Sine_Data[angle+0x80]));
     word sinval = (*(word*)(&Sine_Data[angle]));
     

     cosine=(signed short)SWAPWORD(cosval);
     sine=(signed short)SWAPWORD(sinval);
     //cosine=(cos(((angle)*1.40625)*0.0174532925)*256);
     //sine=(sin(((angle)*1.40625)*0.0174532925)*256);
}

//RandomNumber:
long RandomNumber()
{
    //move.l	(v_random).w,d1
    long random = v_random;

    //bne.s	@scramble	; if d0 is not 0, branch
    if (random == 0)
    {	
        //move.l	#$2A6D365A,d1	; if do is 0, use seed number
        random = 0x2A6D365A;
    }
	
    //@scramble:
    //move.l	d1,d0
    long oldRandom = random;

    //asl.l	#2,d1
    long shiftRandom = random << 2;

    //add.l	d0,d1
    long newRandom = shiftRandom + oldRandom;

    //asl.l	#3,d1
    long shiftNewRandom = newRandom << 3;

    //add.l	d0,d1
    long moreRandom = shiftNewRandom + shiftRandom;

    //move.w	d1,d0
    word lessRandom = ((moreRandom & 0xFFFF0000) >> 16);
 
    //swap	d1
    moreRandom = ((moreRandom & 0x0000FFFF)<<16) +  ((moreRandom & 0xFFFF0000)>>16);

    //add.w	d1,d0
    word moreOrLessRandom = lessRandom + ((moreRandom & 0xFFFF0000)>>16);

    //move.w	d0,d1
    //swap	d1
    long mostlyRandom = ((moreRandom & 0x0000FFFF)<<16) + ((moreOrLessRandom & 0xFFFF0000)>>16);

    //move.l	d1,(v_random).w
    v_random = mostlyRandom;

    return mostlyRandom;
}   //rts	
//; ---------------------------------------------------------------------------
//; Oscillating number subroutines
//; ---------------------------------------------------------------------------

//; ||||||||||||||| S U B	R O U T	I N E |||||||||||||||||||||||||||||||||||||||

//; Initialise the values

const word directionField = 0x007C;     //000000001111100

//OscillateNumInit:			; XREF: GM_Level
void OscillateNumInit()
{
    //lea	(v_oscillate).w,a1
    word *oscPtr = (word*)v_oscillate;

    //lea	(@baselines).l,a2
    word *baselinesPtr = (word*)baselines;   

    //moveq	#$20,d1
    long count = 31;

    *(oscPtr++) = directionField;

    //@loop:
    for (;count>=0; count--)
    {  
        word baseline = *(baselinesPtr++);
        //move.w	(a2)+,(a1)+	; copy baseline values to RAM
        *(oscPtr++) = SWAPWORD(baseline); 
    }   //dbf	d1,@loop

    //rts	
}   //; ===========================================================================


//@baselines:	dc.b %00000000		; oscillation direction bitfield
//		dc.b %01111100
//		incbin	"misc\Oscillating Number Baselines.bin"
//even

//; ||||||||||||||| S U B	R O U T	I N E |||||||||||||||||||||||||||||||||||||||

//; Oscillate values

#include "object.h"
#define player objects[0]
extern object *objects;

word settings[32] = { 2, 0x10,  // ; frequency, amplitude
		      2, 0x18,
		      2, 0x20,
		      2, 0x30,
		      4, 0x20,
		      8,    8, 
		      8, 0x40,
		      4, 0x40,
		      2, 0x50,
		      2, 0x50,
		      2, 0x20,
		      3, 0x30,
		      5, 0x50,
		      7, 0x70,
		      2, 0x10,
		      2, 0x10
};

//OscillateNumDo:				; XREF: GM_Level
void OscillateNumDo()
{
    //cmpi.b	#6,(v_player+obRoutine).w ; has Sonic just died?
    //bcc.s	@end                      ; if yes, branch
    if (player.Routine < 6)
    {				
        //lea	(v_oscillate).w,a1
        word *dPtr = (word*)v_oscillate;

	//lea	(@settings).l,a2
        word *settingsPtr = settings;

	//move.w	(a1)+,d3	; get oscillation direction bitfield
	word directionBits = *(dPtr++);
	word *oscPtr = dPtr;
         
        //moveq	#$F,d1     
        //@loop:
        for (long oscIndex=15; oscIndex>=0; oscIndex--)
        {
	    //move.w	(a2)+,d2	; get frequency
            word frequency = *(settingsPtr++);

	    //move.w	(a2)+,d4	; get amplitude
            word amplitude = *(settingsPtr++);
		
            //btst	d1,d3		; check oscillation direction
            //bne.s	@down		; branch if 1
            if ((directionBits & (1<<oscIndex)) == 0)
            { 
	        //@up:
		//move.w	2(a1),d0
                signed short curPhase = oscPtr[1];

		//add.w	d2,d0
                signed short newPhase = curPhase + frequency;     //new = old + increase rate, linear function with slope=increase rate

		//move.w	d0,2(a1)
                oscPtr[1] = newPhase;              //linear     y = mx + b, y = oscPtr[1], m = frequency, b=base value (x is time)

		//add.w	d0,0(a1)
                oscPtr[0] = oscPtr[0] + newPhase;   //exponential   = new + old = sum(y sub x = mx), y0=mx, y1=y0+m(x+1)

		//cmp.b	0(a1),d4
                //bhi.s	@next
                if (amplitude <= (oscPtr[0]>>8))
                {
		    //bset	d1,d3
		    directionBits = directionBits | (1<<oscIndex);
                    
                }   //bra.s	@next
            }
	    else
            {
                //@down:
		//move.w	2(a1),d0
                signed short curPhase = oscPtr[1];

		//sub.w	d2,d0
                signed short newPhase = curPhase - frequency;     //new = old - increase rate, linear function with slope=decrease rate
            
		//move.w	d0,2(a1)
                oscPtr[1] = newPhase;              //linear     y = -mx + b, y = oscPtr[1], m = frequency, b=base value (x is time)
		
                //add.w	d0,0(a1)
                oscPtr[0] = oscPtr[0] + newPhase;  //exponential   = new + old = sum(y sub x = mx), y0=mx, y1=y0+m(x+1)
		
                //cmp.b	0(a1),d4
                //bls.s	@next
                if (amplitude >= (oscPtr[0]>>8))
                {
		    //bclr	d1,d3
                    directionBits = directionBits - (directionBits & (1<<oscIndex));
                }
            }

	    //@next:
	    //addq.w	#4,a1
            oscPtr = oscPtr + 2;    //2 words, for the linear and exponential growth portions respectively

	}   //dbf	d1,@loop
            
        //move.w	d3,(v_oscillate).w
        ((word*)v_oscillate)[0] = directionBits;
    }
    
    //@end:
    //rts	
}   //; End of function OscillateNumDo

//; ===========================================================================


